from px4_msgs.msg import VehicleStatus
import enum
from geopy.distance import geodesic
import math
import numpy as np
import json

from sls_controller.sls_missionsABC import Missions, MissionsState
from sls_mirrors.sls_agent import ScheduleUpdate
from libs.sls_mirrors import sls_agent
from libs.sls_mirrors.sls_agent import Mirror
from sls_controller.sls_publicDefine import ParamDefault

        

class MissionsTakeoff(Missions):
    def __init__(self,publicResource,datasets:dict={"takeoff_relative_altitude": 3.0,"yaw":None}):
        super().__init__(publicResource,datasets)
        self.takeoffPosition = None
    def fore_check(self):
        if self.foreCheckTimes == -1:         
            if self.publicResource.uav_resource.is_arming_status_armed():
                return True
            else:
                self.publicResource.uav_resource.set_arm()
                return False

    def done_check(self):
        #print(self.publicResource.uav_resource.get_relative_altitude()["NED"])
        if self.publicResource.uav_resource.get_relative_altitude()["NED"] >= self.datasets["takeoff_relative_altitude"] - 0.5:
            return True
        else:
            return False
        
    def mission_action(self):
        self.publicResource.uav_resource.set_vehicle_position(self.takeoffPosition["N"],self.takeoffPosition["E"],self.takeoffPosition["D"]-self.datasets["takeoff_relative_altitude"],self.datasets["yaw"])
    def mission_enter(self):
        self.publicResource.uav_resource.set_position_control()
        #record takeoff position
        self.publicResource.uav_resource.record_takeoff_GNSS_position()
        #record takeoff position
        self.publicResource.uav_resource.record_takeoff_NED_position()
        #record takeoff altitude
        self.publicResource.uav_resource.record_takeoff_altitude()
        #record takeoff timestamp
        self.publicResource.uav_resource.record_takeoff_timestamp()
        self.takeoffPosition = self.publicResource.uav_resource.get_takeoff_NED_position()
        if self.datasets["yaw"] is None:
            self.datasets["yaw"] = self.publicResource.uav_resource.get_vehicle_yaw()


    def mission_exit(self):
        pass

    def abort_check(self):
        pass
    
    def mission_abort(self):
        pass

class MissionsLand(Missions):
    def __init__(self,publicResource,datasets:dict={"landing_detection_time": 1,"yaw":None}):
        super().__init__(publicResource,datasets)
        self.landing_detection_time = datasets["landing_detection_time"]
        self.landing_detection_times = self.landing_detection_time / self.publicResource.uav_resource.get_main_timer_during()
        self.landing_detection_counts = 0
    
    def fore_check(self):
        if self.foreCheckTimes == -1:         
            return self.publicResource.uav_resource.is_arming_status_armed()
    
    def done_check(self):
        if self.publicResource.uav_resource.get_NED_velocity()["D"] <= 0.05:
            self.landing_detection_counts += 1
            if self.landing_detection_counts >= self.landing_detection_times:
                #self.publicResource.disarm()
                return True
        else:
            self.landing_detection_counts = 0
        return False 
        
    def mission_action(self):
        self.publicResource.uav_resource.set_vehicle_velocity(0.0,0.0,0.5,self.datasets["yaw"])
    
    def mission_enter(self):
        self.publicResource.uav_resource.set_velocity_control()
        if self.datasets["yaw"] is None:
            self.datasets["yaw"] = self.publicResource.uav_resource.get_vehicle_yaw()

    def mission_exit(self):
        pass

    def abort_check(self):
        pass  

    
    def mission_abort(self):
        pass   

class MissionsWaypoint(Missions):
    def __init__(self,publicResource,datasets:dict={"latitude":47.3978,"longitude":8.5461}):
        super().__init__(publicResource,datasets)
    
    def fore_check(self):
        if self.foreCheckTimes == -1:
            if self.publicResource.uav_resource.is_flying_status():
                return True
            else:
                return False
    
    def done_check(self):
        if self.distance <=1:
            return True
        else:
            return False
    def vehicle_gps_position_callback():
        pass
         
    def mission_action(self):
        self.sourceWGSPoint = [self.publicResource.uav_resource.get_GNSS_position()["latitude"],self.publicResource.uav_resource.get_GNSS_position()["longitude"]]
        #计算离目标的距离和角度
        self.distance =  geodesic(self.destWGSPoint,self.sourceWGSPoint).m
        angleFromNoth = self.calculate_bearing(self.sourceWGSPoint,self.destWGSPoint) /180.0*math.pi
        #考虑到GPS的精度是米级，所以当大于1m时才进行速度控制
        if self.distance > 1:
            self.speed = self.distance*0.1
            if self.speed > 5:
                self.speed = 5
            self.publicResource.uav_resource.set_vehicle_velocity(float(self.speed*math.cos(angleFromNoth)),float(self.speed*math.sin(angleFromNoth)),0.0,None)
        else:
            self.publicResource.uav_resource.set_vehicle_velocity(0.0,0.0,0.0,None)
    
    def mission_enter(self):
        self.publicResource.uav_resource.set_velocity_control()
        self.destWGSPoint = (self.datasets["latitude"],self.datasets["longitude"])

    def mission_exit(self):
        pass       
    
    def abort_check(self):
        pass   
   
    def mission_abort(self):
        pass  
    def calculate_bearing(self, pos1, pos2):
        """
        计算从点1(lat1, lon1)到点2(lat2, lon2)的方位角。
        方位角以度为单位，从北方向顺时针测量。
        """
        lat1, lon1 = pos1
        lat2, lon2 = pos2
        # 将经纬度从度转换为弧度
        lat1_rad = math.radians(lat1)
        lon1_rad = math.radians(lon1)
        lat2_rad = math.radians(lat2)
        lon2_rad = math.radians(lon2)

        # 计算经度差
        delta_lon = lon2_rad - lon1_rad

        # 使用球面三角学公式计算方位角
        x = math.sin(delta_lon) * math.cos(lat2_rad)
        y = math.cos(lat1_rad) * math.sin(lat2_rad) - (math.sin(lat1_rad) * math.cos(lat2_rad) * math.cos(delta_lon))

        initial_bearing = math.atan2(x, y)

        # 将方位角从弧度转换为度，并调整为0-360度范围
        initial_bearing_degrees = math.degrees(initial_bearing)
        compass_bearing = (initial_bearing_degrees + 360) % 360

        return compass_bearing

from sls_controller.sls_libs import QRland 
from sls_msgs.msg import SLSApirltag
from std_msgs.msg import Int16


class MissionsQRLand(Missions):
    def __init__(self,publicResource,datasets:dict=None):
        super().__init__(publicResource,datasets)
        self.camera_coordinate_subscriber = self.publicResource.ros_resource.create_subscription(
            SLSApirltag,'apirltag_coordination',self.apirltag_coordination_callback,10)
#       self.laser_dis_subscriber = self.publicResource.ros_resource.create_subscription(
#            Int16,'/sls/sensors/height_laser/out/dist_measure',self.laser_dis_callback,2)

        self.x = 0.0
        self.y = 0.0
        self.angle = 0.0
        self.isdetect = 0
        self.Hdist = -1

        self.CameraCoord = QRland.CameraCalibration()
        self.CameraCoord.get_CamParam(centerX=320, centerY=240, focus=320)
        self.RotMarTrans = QRland.TransfMtx()
        self.N_axis = QRland.PID(kp=0.6, ki=0.015, kd=0.0)
        self.E_axis = QRland.PID(kp=0.6, ki=0.015, kd=0.0)
        self.RealE = 100.0
        self.RealN = 100.0
        self.H = -100.0
        self.Last_high = -1
        self.LaserH = -1
        self.get_current_yaw = self.publicResource.uav_resource.get_vehicle_yaw
        self.temp_pos = None
    def fore_check(self):
        return True
    
    def done_check(self):
        self.LaserH = self.get_laser_dis()
        if 0< self.LaserH < 200:
            self.publicResource.ros_resource.destroy_subscription(self.camera_coordinate_subscriber)
            return True
        else:
            return False
        
    def mission_action(self):
        self.LaserH = self.get_laser_dis() if self.get_laser_dis() < self.LaserH else self.LaserH
        # 检测到二维码
        if self.isdetect:
            if self.id == 0:
                self.N_axis = QRland.PID(kp=0.7, ki=0.015, kd=0.0)
                self.E_axis = QRland.PID(kp=0.7, ki=0.015, kd=0.0)
            if self.id == 10:
                self.N_axis = QRland.PID(kp=0.7, ki=0.03, kd=0.0)
                self.E_axis = QRland.PID(kp=0.7, ki=0.03, kd=0.0)
                self.N_axis.integral = 0
                self.E_axis.integral = 0

            # get three-axis attitude
            roll, pitch, yaw = self.RotMarTrans.quaternion2euler(self.publicResource.uav_resource.get_q_FRD_REFTO_NED())

            # 高度传感器0-20000mm为有效数据
            if 0 < self.LaserH <20000:
                high = self.LaserH/1000.0
                self.Last_high = self.LaserH
            # 激光测距信号丢失
            elif self.LaserH < 0:
                # 上次没丢失，取上次测距高度
                if self.Last_high >0:
                    high = self.Last_high/1000
                    self.Last_high = -1
                # 连续两次丢失，取PX4高度
                else:
                    high = self.publicResource.uav_resource.get_relative_altitude()["NED"]
                    high = high/math.cos(roll)/math.cos(pitch)
            # 测距高度不在 0-8000mm 范围，取PX4高度
            else:
                high = self.publicResource.uav_resource.get_relative_altitude()["NED"]
                high = high/math.cos(roll)/math.cos(pitch)
            # get the vertical length from camera to the land
            self.CameraCoord.get_CamHigh(high)
            # convert image coordination to camera physical coordination
            camX,camY = self.CameraCoord.Img2Camera(self.x, self.y)
            # convert camera relative physical coordination to absoluteness
            CoordVector = np.array([[camX],[camY],[high]])
            convert = np.dot(self.RotMarTrans.MulMtx(roll,pitch,yaw),CoordVector)

            self.RealN = convert[0][0]
            self.RealE = convert[1][0]
            self.H  = convert[2][0]
            self.N_velocity = self.N_axis.calculate(current=self.RealN, err_threshold=3, integral_threshold=10)
            self.E_velocity = self.E_axis.calculate(current=self.RealE, err_threshold=3, integral_threshold=10)
            # print(f'E_velocity:{self.E_velocity},N_velocity:{self.N_velocity}')
            if self.N_velocity > 1.5:
                self.N_velocity = 1.5
            if self.E_velocity > 1.5:
                self.E_velocity = 1.5
            if self.N_velocity < -1.5:
                self.N_velocity = -1.5
            if self.N_velocity < -1.5:
                self.N_velocity = -1.5

            if abs(self.RealN) <0.3 and abs(self.RealE) <0.3 and self.H >-99.0 and self.LaserH >= 550:
                self.publicResource.uav_resource.set_vehicle_velocity(self.N_velocity,self.E_velocity,0.5,0.0)
            elif abs(self.RealN) <0.06 and abs(self.RealE) <0.06 and self.H >-99.0 and self.id == 10 and self.LaserH < 550:
                self.publicResource.uav_resource.set_vehicle_velocity(self.N_velocity,self.E_velocity,0.03,0.0)
            else:
                self.publicResource.uav_resource.set_vehicle_velocity(self.N_velocity, self.E_velocity, 0.0, 0.0)
            print(f"E:{self.RealE},N:{self.RealN},Z:{self.H}")
            print(f"VE:{self.E_velocity},VN:{self.N_velocity}")
            print(self.H)
            self.temp_pos = None
        else:
            if self.temp_pos is None:
                self.temp_pos = self.publicResource.uav_resource.get_NED_position()
            else:
                self.publicResource.uav_resource.set_vehicle_position(self.temp_pos["N"],self.temp_pos["E"],self.temp_pos["D"],self.get_current_yaw())
    def mission_enter(self):
        pass

    def mission_exit(self):
        pass

    def abort_check(self):
        pass  

    
    def mission_abort(self):
        pass

    def apirltag_coordination_callback(self, data=SLSApirltag):
        self.x = data.x * 1.0
        self.y = data.y * 1.0
        self.angle = data.angle * 1.0
        self.isdetect = data.isdetect
        self.id = data.id

    def get_laser_dis(self):
        if self.publicResource.uav_resource.get_laser_altitude() < 0:
            return self.publicResource.uav_resource.get_laser_altitude()
        else:
            return int(self.publicResource.uav_resource.get_laser_altitude()*1000)

class MissionsAddUGVTasks(Missions):
    def __init__(self, publicResource, datasets: dict = None):
        super().__init__(publicResource, datasets)
    def fore_check(self):
        return True
    def done_check(self):
        self.publicResource.mirrors_agent.remove_mirrors(self.ugv_mirror1)
        return True
    def mission_action(self):
        pass
    def mission_enter(self):
        self.ugv_mirror1 = sls_agent.Mirror("ugv1")
        self.publicResource.mirrors_agent.add_mirrors(self.ugv_mirror1)
        for mission in self.datasets:
            self.publicResource.mirrors_agent.append_mission_to_schedule("ugv1",mission)

    def mission_exit(self):
        pass
    def abort_check(self):
        pass
    def mission_abort(self):
        pass
class MissionsUAVFinishedFlag(Missions):
    finish_flag = False
    def __init__(self, publicResource, datasets: dict = None):
        super().__init__(publicResource, datasets)
    def fore_check(self):
        return True
    def done_check(self):
        MissionsUAVFinishedFlag.finish_flag = True
        return True
    def mission_action(self):
        pass
    def mission_enter(self):
        return True
    def mission_exit(self):
        pass
    def abort_check(self):
        pass
    def mission_abort(self):
        pass


from lifecycle_msgs.srv import ChangeState
from lifecycle_msgs.msg import Transition
import time,rclpy
from rclpy.client import Client
class MissionsUAVLifecycleActivate(Missions):
    def __init__(self, publicResource, datasets: list):
        super().__init__(publicResource, datasets)
        self.nodes = datasets
        self.clients = []
    def fore_check(self):
        return True

    def done_check(self):
        for client in self.clients:
            self.publicResource.ros_resource.destroy_client(client)
        return True

    def mission_action(self):
        pass
    
    def mission_enter(self):
        for node in self.nodes:
            client = self.publicResource.ros_resource.create_client(
                ChangeState, f"/{node}/change_state"
            )
            self.clients.append(client)
            while not client.wait_for_service(timeout_sec=1.0):
                print((f"节点{node}不存在"))
                self.clients.remove(client)
                break
        for client in self.clients:
            client:Client
            configure_request = ChangeState.Request()
            configure_request.transition.id = Transition.TRANSITION_CONFIGURE
            configure_future = client.call_async(configure_request)
            rclpy.spin_until_future_complete(self.publicResource.ros_resource.main_node, configure_future)
            
            if configure_future.result() is None or not configure_future.result().success:
                return False
            else:
                activate_request = ChangeState.Request()
                activate_request.transition.id = Transition.TRANSITION_ACTIVATE
                activate_future = client.call_async(activate_request)
                rclpy.spin_until_future_complete(self.publicResource.ros_resource.main_node, activate_future)
                
                if activate_future.result() is None or not activate_future.result().success:
                    return False
        return True

    def mission_exit(self):
        pass

    def abort_check(self):
        pass
    
    def mission_abort(self):
        pass


class MissionsLifecycleDeactivate(Missions):
    def __init__(self, publicResource, datasets: dict = None):
        super().__init__(publicResource, datasets)

    def fore_check(self):
        return True

    def done_check(self):
        self.publicResource.ros_resource.destroy_client(ChangeState,'/lc_talker/change_state')
        return True

    def mission_action(self):
        pass
    
    def mission_enter(self):
        self.client = self.publicResource.ros_resource.create_client(ChangeState, '/lc_talker/change_state')
        while not self.client.wait_for_service(timeout_sec=1.0):
            pass
        request = ChangeState.Request()
        request.transition.id = Transition.TRANSITION_DEACTIVATE

        future = self.client.call_async(request)
        rclpy.spin_until_future_complete(self.publicResource.ros_resource.main_node, future)
        if future.result() is not None and future.result().success:
            print('ok')
        pass

    def mission_exit(self):
        pass

    def abort_check(self):
        pass
    
    def mission_abort(self):
        pass

class MissionsHover(Missions):
    def __init__(self, publicResource, datasets:dict={"hover_duration": 10.0,"target_altitude": 8.0}):
        super().__init__(publicResource, datasets)
        self.datasets = datasets or {"hover_duration": 10.0, "target_altitude": 8.0}
        self.hover_start_count = 0
        self.hover_duration_counts = int(self.datasets["hover_duration"] / ParamDefault.MAIN_TIMER_DURING)
        self.target_position = None
        self.altitude_reached = False  
        self.target_altitude = self.datasets.get("target_altitude", 5.0)       

    def fore_check(self):
        if self.foreCheckTimes == -1:
            if self.publicResource.uav_resource.is_flying_status():
                return True
            else:
                self.publicResource.uav_resource.set_arm()
                return False

    def done_check(self):
        if self.hover_start_count >= self.hover_duration_counts:
            return True
        else:
            return False
        
    def mission_enter(self):
        self.publicResource.uav_resource.set_position_control()
        self.altitude_reached = False
        self.hover_start_count = 0

        current_position = self.publicResource.uav_resource.get_NED_position()
        if current_position:
            self.target_position = current_position.copy()

    def mission_action(self):
        current_position = self.publicResource.uav_resource.get_NED_position()

        current_altitude = float(self.publicResource.uav_resource.get_relative_altitude()["NED"])
        
        if not self.altitude_reached:
            if abs(current_altitude - self.target_altitude) <= 0.3:
                self.altitude_reached = True
            else:
                altitude_diff = self.target_altitude - current_altitude
                target_down = current_position.get('D', 0) - altitude_diff

                self.publicResource.uav_resource.set_vehicle_position(
                    current_position.get('N', 0),  
                    current_position.get('E', 0),  
                    target_down,                   
                    0.0                           
                )
        else:
            current_gnss_altitude = float(self.publicResource.uav_resource.get_relative_altitude()["NED"])

            altitude_diff = self.target_altitude - current_gnss_altitude
            target_down = current_position.get('D', 0) - altitude_diff * 0.5  # 使用比例控制

            self.publicResource.uav_resource.set_vehicle_position(
                self.target_position.get('N', 0),  
                self.target_position.get('E', 0),  
                target_down,  
                0.0  
            )

            self.hover_start_count += 1

    def mission_exit(self):
        pass

    def abort_check(self):
        pass
    
    def mission_abort(self):
        pass
